// Fill out your copyright notice in the Description page of Project Settings.

#pragma once

#include "WorldMath.h"
#include <memory>
#include <vector>

/**
 * An intermediate representation of a reference line in the form of cubic hermite spline.
 * IMPORTANT: distance and length are all in 2D xy-plane, matching OpenDRIVE spec.
 */
class FWorldRefLineSpline
{

public:

	// Reference: https://en.wikipedia.org/wiki/Cubic_Hermite_spline
	struct Interval
	{
		glm::dvec3	Param;	// Param. S (start distance) and L (interval length)
		glm::dvec3	P0;		// Incoming point.
		glm::dvec3	M0;		// Incoming tangent.
		glm::dvec3	P1;		// Outgoing point.
		glm::dvec3	M1;		// Outgoing tangent.
	};

	// S (distance) -> t (param) mapping.
	struct Reparam
	{
		double	S;		// S: distance along curve
		double	T;		// T: parameter in range IntervalIndex+[0,1]
	};

	using Ptr = std::shared_ptr<FWorldRefLineSpline>;

public:

	void Add(const glm::dvec3& param, const glm::dvec3& p0, const glm::dvec3& m0, const glm::dvec3& p1, const glm::dvec3& m1);
	void Add(const Interval& interval);

	bool IsEmpty() const;
	int IntervalCount() const;
	double Length() const;

	void UpdateReparam(int intervalSteps = 10);
	double GetIntervalLength(int index, double t) const;
	int FindInterval(double t) const;
	int FindIntervalAtDistance(double s) const;
	double FindParamAtDistance(double s) const;

	glm::dvec3 Eval(double t) const;
	glm::dvec3 EvalAtDistance(double s) const;

	glm::dvec3 EvalTangent(double t) const;
	glm::dvec3 EvalTangentAtDistance(double s) const;

	const std::vector<Interval>& Get() const;
	std::vector<Interval>& Get();

	FWorldRefLineSpline::Ptr Clone() const;

private:

	std::vector<Interval>		_spline;
	std::vector<Reparam>		_reparam;

};
